mat_contents = sio.loadmat('data_odometry.mat')
X = np.array(mat_contents["X"][0])
Y = np.array(mat_contents["Y"][0])
V_right = np.array(mat_contents["V_right"].transpose()[0])
V_left = np.array(mat_contents["V_left"].transpose()[0])
V_odom = (V_right + V_left) / 2
fig, ax = plt.subplots(1, 1, figsize=(6, 3), dpi=600)
ax.set_title("GPS trajectory")
ax.plot(X, Y)
ax.set_ylabel("Y")
ax.set_xlabel("X");
fig, ax = plt.subplots(1, 2, figsize=(6, 3), dpi=600)
ax[0].plot(X);ax[0].set_title("X from GPS");ax[0].set_ylabel("X");ax[0].set_xlabel("points");ax[1].plot(Y);ax[1].set_title("Y from GPS");ax[1].set_ylabel("Y");ax[1].set_xlabel("points");
plt.tight_layout();
fig, ax = plt.subplots(1, 1, figsize=(6, 3), dpi=600)
ax.patch.set_facecolor('grey');ax.set_title("Velocity");ax.plot(V_left,color='white',label=r'$V_{left}$');ax.plot(V_odom,color='blue',label=r"$V_{odometry}$");ax.plot(V_right,color='red',label=r'$V_{right}$');ax.set_xlabel("points");ax.set_ylabel("V");
ax.legend(loc='best');
$$v_i = \frac{x_{i+1} - x_{i-1}}{2 t}$$
def velocity_from_traj(x, t):
v = (np.roll(x, -1) - np.roll(x, 1)) / 2 / t
v[-1] = v[-2]
v[0] = v[1]
return v
t = 0.05
V_x = velocity_from_traj(X, t)
V_y = velocity_from_traj(Y, t)
V_GPS = (V_x ** 2 + V_y ** 2) ** 0.5
fig, ax = plt.subplots(1, 3, figsize=(6, 3), dpi=600);
ax[0].plot(V_x,label='$V_x$');ax[1].set_title('GPS');ax[0].set_ylabel("Velocity");ax[1].plot(V_y,label="$V_y$");ax[2].plot(V_GPS,label=r"$\left|V\right|$");
for axe in ax:
axe.legend(loc='lower center');axe.set_xlabel("points");
fig.tight_layout();
def LR(y,x):
N = len(y)
β = inv(x.dot(x.transpose())).dot(x.dot(y.transpose()))
return β
def RMS(y,x,β):
return (np.sum((y-x.transpose().dot(β))**2)/ (len(y)-1)) ** 0.5
y = V_GPS
x = V_odom
β = LR(y,np.array([np.ones_like(x),x]))
print(β)
rms = RMS(y, np.array([np.ones_like(x),x]), β)
print(rms)
[ 2.01151947 0.79883512] 0.0851295301498
$$V_{GPS} = 2.01 + 0.8 \cdot V_{odometry}$$ $$RMS = 0.085$$
fig, ax = plt.subplots(1, 1, figsize=(6, 3), dpi=600);ax.scatter(x, y, s=0.3,label='GPS data');x_line = x;y_line = np.array([np.ones_like(x),x]).transpose().dot(β);ax.plot(x_line, y_line, linewidth=1, color="r",label='linear fit');ax.legend(loc='best');ax.set_xlabel("$V_{odom}$");ax.set_ylabel("$V_{GPS}$");
ax.set_title("Linear fitting with RMS = %.3f" % rms);
sigma_n_2 = 3 ** 2
Xn = X + np.random.normal(0, sigma_n_2 ** 0.5, X.shape[0])
Yn = Y + np.random.normal(0, sigma_n_2 ** 0.5, Y.shape[0])
fig, ax = plt.subplots(1, 2, figsize=(6, 3), dpi=600);ax[0].plot(Xn,label=r'$X_{GPS}$, noisy');ax[0].plot(X, label=r'$X_{GPS}$');ax[0].set_title("X");ax[0].set_ylabel("coordinate");ax[0].legend(loc='lower right');ax[1].plot(Yn,label=r'$Y_{GPS}$, noisy');ax[1].plot(Y, label=r'$Y_{GPS}$');ax[1].set_title("Y");ax[1].legend(loc='lower left');
for axe in ax:
axe.set_xlabel("points")
plt.tight_layout();
Xk, _, P, _ = kalman(X_0, P_0, z[2:], Φ, H, R, Q)
Xs, _, _ = backward_smoothing(Xk, P, Q, Φ)
y = V_gps_noisy[:,0]
x = V_odom[2:]
β = LR(y,np.array([np.ones_like(x),x]))
print(β)
rms = RMS(y, np.array([np.ones_like(x),x]), β)
print(rms)
[ 2.75332442 0.75409233] 0.291461948781
$$V_{GPS} = 2.75 + 0.75 \cdot V_{odometry}$$ $$RMS = 0.29$$
fig, ax = plt.subplots(1, 1, figsize=(6, 3), dpi=600)
ax.scatter(x, y, s=0.3,label='noisy data');ax.scatter(x, V_GPS[2:], s=0.1, label='original');
x_line = x;y_line = np.array([np.ones_like(x),x]).transpose().dot(β);
ax.plot(x_line, y_line, linewidth=1, color="r",label='linear fit');ax.set_xlabel("$V_{odom}$");ax.set_ylabel("$V_{GPS}$");ax.legend(loc='best')
ax.set_title("Linear fit with RMS = %.2f" % rms);
interval = range(50, 450)
V_x_noisy2 = V_x_noisy[interval]
V_y_noisy2 = V_y_noisy[interval]
V_gps_noisy2 = V_gps_noisy[interval]
y = V_gps_noisy2[:,0]
x = V_odom[interval]
β = LR(y,np.array([np.ones_like(x),x]))
print(β)
rms = RMS(y, np.array([np.ones_like(x),x]), β)
print(rms)
[ 2.09250348 0.79628892] 0.196604597478
$$V_{GPS} = 2.09 + 0.8 \cdot V_{odometry}$$ $$RMS = 0.2$$
fig, ax = plt.subplots(1, 1, figsize=(6, 3), dpi=600)
ax.scatter(x, y, s=0.1,label='noisy data');ax.scatter(x, V_GPS[interval], s=0.1, label='original')
x_line = x;y_line = np.array([np.ones_like(x),x]).transpose().dot(β);ax.plot(x_line, y_line, linewidth=1, color="r",label='linear fit');ax.set_xlabel("$V_{odom}$");ax.set_ylabel("$V_{GPS}$");ax.legend(loc='best');ax.set_title("Linear fit with RMS = %.2f" % rms);